class Gesture {
		string name; // name of last gesture
		bool isConfirmed;
		int detectionTime;
		int confirmationTime;
		static const int WINDOW = 2; // size of floating window in s, WINDOW needs to pass before gesture can be confirmed
		static const int EXT_WINDOW = 8; // necessary time in s between to gestures of the same kind
		
		
	public:
		
		bool inGestureArea(FrameSet f) {
			return ((f.right_hand.getZ() > f.right_hip.getZ()) && 
					(f.left_hand.getZ() > f.left_hip.getZ()) && 
					(f.right_hand.getZ() < f.head.getZ()) && 
					(f.left_hand.getZ() < f.head.getZ()));
		}
		
		Frame transform(Frame frame, double dx, double dy, double alpha) {
			// moves frame to origin an removes rotation
			double x = frame.getX() + dx;
			double y = frame.getY() + dy;
			double z = frame.getZ();
			
			double xn = x * cos(alpha) + y * sin(alpha);
			y = -x * sin(alpha) + y * cos(alpha);
			
			string name = frame.getName();
			geometry_msgs::Vector3 vec;
			vec.x = xn;
			vec.y = y;
			vec.z = z;
			
			return Frame(name, vec);
		} 
		
		string whatGesture(FrameSet f) {
			// transformation to body frame
			double alpha = atan2(f.left_shoulder.getX() - f.right_shoulder.getX() , f.right_shoulder.getY() - f.left_shoulder.getY());
			//ROS_INFO("left_shoulder_y = %f, right_shoulder_y = %f", f.left_shoulder.getY(), f.right_shoulder.getY());
			//ROS_INFO("alpha = %f", alpha*180/3.1415);
			double dx = -(f.left_shoulder.getX() + f.right_shoulder.getX()) / 2;
			double dy = -(f.left_shoulder.getY() + f.right_shoulder.getY()) / 2;
			
			Frame right_hand = transform(f.right_hand, dx, dy, alpha);
			Frame left_hand = transform(f.left_hand, dx, dy, alpha);
			Frame right_elbow = transform(f.right_elbow, dx, dy, alpha);
			Frame left_elbow = transform(f.left_elbow, dx, dy, alpha);
			Frame right_shoulder = transform(f.right_shoulder, dx, dy, alpha);
			Frame left_shoulder = transform(f.left_shoulder, dx, dy, alpha);
			
			//ROS_INFO("Y: left hand: %f, right hand: %f", left_hand.getY(), right_hand.getY());
			//ROS_INFO("X: left hand: %f, right hand: %f", left_hand.getX(), right_hand.getX());
			
			if((right_elbow.getY() > left_elbow.getY()) && 
				(right_hand.getY() < left_hand.getY()) && 
				(left_hand.getZ() > right_elbow.getZ()) && 
				(right_hand.getZ() > left_elbow.getZ())) return "stop";
			/*if((right_hand.getY() < right_elbow.getY()) && 
				(left_hand.getY() > left_elbow.getY()) &&
				(right_hand.getY() < left_elbow.getY()) &&
				(left_hand.getZ() < right_elbow.getZ()) && 
				(right_hand.getZ() > left_elbow.getZ())) return "rotate left";
			if((right_hand.getY() < right_elbow.getY()) && 
				(left_hand.getY() > left_elbow.getY()) && 
				(right_elbow.getY() < left_hand.getY()) &&
				(left_hand.getZ() > right_elbow.getZ()) && 
				(right_hand.getZ() < left_elbow.getZ())) return "rotate right";*/
			if((left_hand.getY() < left_elbow.getY()) &&
				(right_hand.getY() < right_elbow.getY()) &&
				(left_elbow.getY() < right_hand.getY()) &&
				(left_hand.getY() < left_shoulder.getY()) &&
				(left_hand.getZ() > left_elbow.getZ()) &&
				(right_hand.getZ() > right_elbow.getZ())) return "move left";
			if((left_hand.getY() > left_elbow.getY()) &&
				(right_hand.getY() > right_elbow.getY()) &&
				(right_elbow.getY() > left_hand.getY()) &&
				(right_hand.getY() > right_shoulder.getY()) &&
				(left_hand.getZ() > left_elbow.getZ()) &&
				(right_hand.getZ() > right_elbow.getZ())) return "move right";
			if((left_elbow.getY() < left_hand.getY()) &&
				(left_hand.getY() < right_hand.getY()) &&
				(right_hand.getY() < right_elbow.getY()) &&
				(left_hand.getX() > left_elbow.getX()) &&
				(right_hand.getX() > right_elbow.getX())) return "move forward";
			if((left_elbow.getY() < left_hand.getY()) &&
				(left_hand.getY() < right_hand.getY()) &&
				(right_hand.getY() < right_elbow.getY()) &&
				(left_elbow.getX() > left_hand.getX()) &&
				(right_elbow.getX() > right_hand.getX())) return "move backward";
			return "";
		} // insert new gesture here
				
		string detectGesture(FrameSet f, int time) {
			string n = whatGesture(f);
			if(!n.empty()) { // gesture detected
				
				if(name.compare(n) == 0) { // last gesture was the same and it has not been confirmed
					if(!isConfirmed) {
						if(detectionTime + WINDOW < time) { // not confirmed and window time passed
							isConfirmed = true;
							confirmationTime = time;
							//ROS_INFO("Gesture %s confirmed.", name.c_str());
							return n;
						}
					} else if(confirmationTime + EXT_WINDOW < time) { 
						// new gesture is the same as last confirmed gesture, see if EXT_WINDOW wait time has passed
						detectionTime = time;
						isConfirmed = false;
						//ROS_INFO("Gesture %s detected.", name.c_str());
					}
				} else { // last gesture was different
					name = n;
					detectionTime = time;
					isConfirmed = false;
					//ROS_INFO("Gesture %s detected.", name.c_str());
				}
			}
			return "";
		}
		
		Gesture() {
			isConfirmed = false;
			detectionTime = 0;
			confirmationTime = 0;
		}
};
